#include <string.h>
#include <stdio.h>

#include <icommon.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
static void c_copy_frame_nv12_to_nv12(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i = 0;
	for (i = 0; i < height; i++) {
		memcpy(dst[0] + i * i_dst_stride[0], src[0] + i * i_src_stride[0], width);
		if (!(i % 2)) {
			memcpy(dst[1] + i / 2 * i_dst_stride[1], src[1] + i / 2 * i_src_stride[1], width);
		}
	}
}

static void c_copy_frame_t420_to_t420(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i_dst_stride_new[2] = {i_dst_stride[0]*16, i_dst_stride[1]*8};
	int i = 0;
	for (i = 0; i < height/16; i++) {
		memcpy(dst[0] + i * i_dst_stride_new[0], src[0] + i * i_src_stride[0], width*16);
		memcpy(dst[1] + i * i_dst_stride_new[1], src[1] + i * i_src_stride[1], width*8);
	}
}

static void c_copy_frame_i420_to_nv12(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i = 0;
	int j = 0;
	pixel *pysrc = NULL;
	pixel *pydst = NULL;
	pixel *pusrc = NULL;
	pixel *pvsrc = NULL;
	pixel *puvdst = NULL;
	pysrc = src[0];
	pydst = dst[0];
	pusrc = src[1];
	pvsrc = src[2];
	puvdst = dst[1];
	for (i = 0; i < height; i++) {
		memcpy(pydst, pysrc, width);
		if (i < height / 2) {
			pixel *tpusrc = pusrc;
			pixel *tpvsrc = pvsrc;
			pixel *tpuvdst = puvdst;
			for (j = 0; j < width / 2; j++) {
				*tpuvdst++ = *tpusrc++;
				*tpuvdst++ = *tpvsrc++;
			}
			pusrc += i_src_stride[1];
			pvsrc += i_src_stride[2];
			puvdst += i_dst_stride[1];
		}
		pysrc += i_src_stride[0];
		pydst += i_dst_stride[0];
	}
}

static void c_copy_frame_nv21_to_nv12(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i = 0;
	int j = 0;
	pixel *pysrc = NULL;
	pixel *pydst = NULL;
	pixel *pvusrc = NULL;
	pixel *puvdst = NULL;
	pysrc = src[0];
	pydst = dst[0];
	pvusrc = src[1];
	puvdst = dst[1];
	for (i = 0; i < height; i++) {
		memcpy(pydst, pysrc, width);
		if (i < height / 2) {
			pixel *tpvusrc = pvusrc;
			pixel *tpuvdst = puvdst;
			for (j = 0; j < width / 2; j++) {
				*tpuvdst++ = *(tpvusrc+1);
				*tpuvdst++ = *tpvusrc;
				tpvusrc += 2;
			}
			pvusrc += i_src_stride[1];
			puvdst += i_dst_stride[1];
		}
		pysrc += i_src_stride[0];
		pydst += i_dst_stride[0];
	}
}

static void c_copy_frame_t420_to_nv12(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i = 0;
	int j = 0;
	int k = 0;
	int l = 0;
	pixel *pysrc = NULL;
	pixel *pydst = NULL;
	pysrc = src[0];
	pydst = dst[0];
	/* copy y info */
	for (i = 0; i < height/16; i++) {
		pixel *tpydst = pydst;
		pixel *tpysrc = pysrc;
		for (j = 0; j < width/16; j++) {
			pixel *ttpydst = tpydst;
			for (k = 0; k < 16; k++) {
				memcpy(ttpydst, tpysrc, 16);
				tpysrc += 16;
				ttpydst += i_dst_stride[0];
			}
			tpydst += 16;
		}
		pysrc += i_src_stride[0]*16;
		pydst += i_dst_stride[0]*16;
	}
	/* copy uv info */
	pixel *pusrc = src[1];
	pixel *puvdst = dst[1];
	for (i = 0; i < height/2/8; i++) {
		pixel *tpusrc = pusrc;
		pixel *tpuvdst = puvdst;
		for (j = 0; j < width/2/8; j++) {
			pixel *ttpusrc = tpusrc;
			pixel *ttpuvdst = tpuvdst;
			for (k = 0; k < 8; k++) {
				pixel *p1 = ttpusrc;
				pixel *p2 = ttpusrc + 8;
				pixel *p3 = ttpuvdst;
				for (l = 0; l < 8; l++) {
					*p3++ = *p1++;
					*p3++ = *p2++;
				}
				ttpusrc += 16;
				ttpuvdst += i_dst_stride[1];
			}
			tpusrc += 16*8;
			tpuvdst += 16;
		}
		pusrc += i_src_stride[1]*8;
		puvdst += i_dst_stride[1]*8;
	}
}

static void c_copy_frame_i420_to_t420(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i_dst_stride_new[2] = {i_dst_stride[0]*16, i_dst_stride[1]*8};
	int i = 0;
	int j = 0;
	int k = 0;
	int l = 0;
	pixel *pysrc = NULL;
	pixel *pydst = NULL;
	pysrc = src[0];
	pydst = dst[0];
	/* copy y info */
	for (i = 0; i < height/16; i++) {
		pixel *tpydst = pydst;
		pixel *tpysrc = pysrc;
		for (j = 0; j < width/16; j++) {
			pixel *ttpysrc = tpysrc;
			for (k = 0; k < 16; k++) {
				memcpy(tpydst, ttpysrc, 16);
				tpydst += 16;
				ttpysrc += i_src_stride[0];
			}
			tpysrc += 16;
		}
		pydst += i_dst_stride_new[0];
		pysrc += i_src_stride[0]*16;
	}
	/* copy uv info */
	pixel *pusrc = src[1];
	pixel *pvsrc = src[2];
	pixel *puvdst = dst[1];
	for (i = 0; i < height/2/8; i++) {
		pixel *tpusrc = pusrc;
		pixel *tpvsrc = pvsrc;
		pixel *tpuvdst = puvdst;
		for (j = 0; j < width/2/8; j++) {
			pixel *ttpusrc = tpusrc;
			pixel *ttpvsrc = tpvsrc;
			pixel *ttpuvdst = tpuvdst;
			for (k = 0; k < 8; k++) {
				pixel *p1 = ttpusrc;
				pixel *p2 = ttpvsrc;
				pixel *p3 = ttpuvdst;
				pixel *p4 = ttpuvdst+8;
				for (l = 0; l < 8; l++) {
					*p3++ = *p1++;
					*p4++ = *p2++;
				}
				ttpusrc += i_src_stride[1];
				ttpvsrc += i_src_stride[2];
				ttpuvdst += 16;
			}
			tpusrc += 8;
			tpvsrc += 8;
			tpuvdst += 16*8;
		}
		pusrc += i_src_stride[1]*8;
		pvsrc += i_src_stride[2]*8;
		puvdst += i_dst_stride_new[1];
	}
}

static void c_copy_frame_nv12_to_t420(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i_dst_stride_new[2] = {i_dst_stride[0]*16, i_dst_stride[1]*8};
	int i = 0;
	int j = 0;
	int k = 0;
	int l = 0;
	pixel *pysrc = NULL;
	pixel *pydst = NULL;
	pysrc = src[0];
	pydst = dst[0];
	/* copy y info */
	for (i = 0; i < height/16; i++) {
		pixel *tpydst = pydst;
		pixel *tpysrc = pysrc;
		for (j = 0; j < width/16; j++) {
			pixel *ttpysrc = tpysrc;
			for (k = 0; k < 16; k++) {
				memcpy(tpydst, ttpysrc, 16);
				tpydst += 16;
				ttpysrc += i_src_stride[0];
			}
			tpysrc += 16;
		}
		pydst += i_dst_stride_new[0];
		pysrc += i_src_stride[0]*16;
	}
	/* copy uv info */
	pixel *puvsrc = src[1];
	pixel *puvdst = dst[1];
	for (i = 0; i < height/2/8; i++) {
		pixel *tpuvsrc = puvsrc;
		pixel *tpuvdst = puvdst;
		for (j = 0; j < width/2/8; j++) {
			pixel *ttpuvsrc = tpuvsrc;
			pixel *ttpuvdst = tpuvdst;
			for (k = 0; k < 8; k++) {
				pixel *p1 = ttpuvsrc;
				pixel *p3 = ttpuvdst;
				pixel *p4 = ttpuvdst+8;
				for (l = 0; l < 8; l++) {
					*p3++ = *p1++;
					*p4++ = *p1++;
				}
				ttpuvsrc += i_src_stride[1];
				ttpuvdst += 16;
			}
			tpuvsrc += 16;
			tpuvdst += 16*8;
		}
		puvsrc += i_src_stride[1]*8;
		puvdst += i_dst_stride_new[1];
	}
}

static void c_copy_frame_nv21_to_t420(pixel *dst[2], int i_dst_stride[2],
			pixel *src[4], int i_src_stride[4], int width, int height)
{
	int i_dst_stride_new[2] = {i_dst_stride[0]*16, i_dst_stride[1]*8};
	int i = 0;
	int j = 0;
	int k = 0;
	int l = 0;
	pixel *pysrc = NULL;
	pixel *pydst = NULL;
	pysrc = src[0];
	pydst = dst[0];
	/* copy y info */
	for (i = 0; i < height/16; i++) {
		pixel *tpydst = pydst;
		pixel *tpysrc = pysrc;
		for (j = 0; j < width/16; j++) {
			pixel *ttpysrc = tpysrc;
			for (k = 0; k < 16; k++) {
				memcpy(tpydst, ttpysrc, 16);
				tpydst += 16;
				ttpysrc += i_src_stride[0];
			}
			tpysrc += 16;
		}
		pydst += i_dst_stride_new[0];
		pysrc += i_src_stride[0]*16;
	}
	/* copy uv info */
	pixel *puvsrc = src[1];
	pixel *puvdst = dst[1];
	for (i = 0; i < height/2/8; i++) {
		pixel *tpuvsrc = puvsrc;
		pixel *tpuvdst = puvdst;
		for (j = 0; j < width/2/8; j++) {
			pixel *ttpuvsrc = tpuvsrc;
			pixel *ttpuvdst = tpuvdst;
			for (k = 0; k < 8; k++) {
				pixel *p1 = ttpuvsrc;
				pixel *p3 = ttpuvdst;
				pixel *p4 = ttpuvdst+8;
				for (l = 0; l < 8; l++) {
					*p4++ = *p1++;
					*p3++ = *p1++;
				}
				ttpuvsrc += i_src_stride[1];
				ttpuvdst += 16;
			}
			tpuvsrc += 16;
			tpuvdst += 16*8;
		}
		puvsrc += i_src_stride[1]*8;
		puvdst += i_dst_stride_new[1];
	}
}

int c_mc_init(c_mc_function_t *mc)
{
	mc->copy_frame[0] = c_copy_frame_nv12_to_nv12;
	mc->copy_frame[1] = c_copy_frame_i420_to_nv12;
	mc->copy_frame[2] = c_copy_frame_nv21_to_nv12;
	mc->copy_frame[3] = c_copy_frame_t420_to_nv12;
	mc->copy_frame[4] = c_copy_frame_nv12_to_t420;
	mc->copy_frame[5] = c_copy_frame_i420_to_t420;
	mc->copy_frame[6] = c_copy_frame_nv21_to_t420;
	mc->copy_frame[7] = c_copy_frame_t420_to_t420;

	return 0;
}

int c_mc_type(c_csp_t dst, c_csp_t src)
{
	int ret = 0;

	if (dst == C_CSP_NV12) {
		switch (src) {
		case C_CSP_NV12:	ret = NV12_TO_NV12; break;
		case C_CSP_I420:	ret = I420_TO_NV12; break;
		case C_CSP_NV21:	ret = NV21_TO_NV12; break;
		case C_CSP_T420:	ret = T420_TO_NV12; break;
		default: break;
		}
	} else {	/* dst == C_CSP_T420 */
		switch (src) {
		case C_CSP_NV12:	ret = NV12_TO_T420; break;
		case C_CSP_I420:	ret = I420_TO_T420; break;
		case C_CSP_NV21:	ret = NV21_TO_T420; break;
		case C_CSP_T420:	ret = T420_TO_T420; break;
		default: break;
		}
	}

	return ret;
}
